import matplotlib.pyplot as plt
import matplotlib.ticker as ticker
from matplotlib.patches import Ellipse, Circle
import numpy as np


def plot2DBase(obstacle, pathList, startPoint, endPoint, length, width):  # 无人车A*算法
    plt.figure(1)
    mazerun = plt.gca()
    mazerun.set_xlim(0, length - 1)
    mazerun.set_ylim(0, width - 1)
    mazerun.xaxis.set_major_locator(ticker.MaxNLocator(length - 1))
    mazerun.yaxis.set_major_locator(ticker.MaxNLocator(width - 1))

    for i in obstacle:  # 画出障碍物和起点终点
        plt.fill_between([i[0], i[0] + 1], i[1], i[1] + 1, color='black')
    plt.fill_between([startPoint[0], startPoint[0] + 1], startPoint[1], startPoint[1] + 1, color='green')
    plt.fill_between([endPoint[0], endPoint[0] + 1], endPoint[1], endPoint[1] + 1, color='red')

    for i in range(len(pathList)):  # 画出路径点与连线
        plt.plot(pathList[i][0], pathList[i][1], '-k.')
        if i < len(pathList) - 1:
            plt.plot([pathList[i][0], pathList[i + 1][0]], [pathList[i][1], pathList[i + 1][1]], 'r')

    plt.grid()  # 生成网格
    plt.show()


def plotMaze(obstacle, startPoint, endPoint, length, width):
    if len(obstacle[0]) == 2:
        plt.figure(1)
        mazerun = plt.gca()
        mazerun.set_xlim(0, length - 1)
        mazerun.set_ylim(0, width - 1)
        mazerun.xaxis.set_major_locator(ticker.MaxNLocator(length - 1))
        mazerun.yaxis.set_major_locator(ticker.MaxNLocator(width - 1))

        for i in obstacle:  # 画出障碍物和起点终点
            plt.fill_between([i[0], i[0] + 1], i[1], i[1] + 1, color='black')
        plt.fill_between([startPoint[0], startPoint[0] + 1], startPoint[1], startPoint[1] + 1, color='green')
        plt.fill_between([endPoint[0], endPoint[0] + 1], endPoint[1], endPoint[1] + 1, color='red')

        plt.grid()  # 生成网格
        plt.show()

    elif len(obstacle[0]) == 3:
        plt.figure(1)
        mazerun = plt.gca()
        mazerun.cla()
        mazerun.set_xlim(0, length - 1)
        mazerun.set_ylim(0, width - 1)
        mazerun.xaxis.set_major_locator(ticker.MaxNLocator(length - 1))
        mazerun.yaxis.set_major_locator(ticker.MaxNLocator(width - 1))

        for i in obstacle:  # 画出障碍物和起点终点
            circles = plt.Circle((i[0], i[1]), i[2], color='black')
            mazerun.add_artist(circles)
        plt.fill_between([startPoint[0], startPoint[0] + 1], startPoint[1], startPoint[1] + 1, color='green')
        plt.fill_between([endPoint[0] - 1, endPoint[0]], endPoint[1] - 1, endPoint[1], color='red')
        plt.show()

def plotSparse(obstacle, pathList, startPoint, endPoint, length, width):  # 无人机稀疏步长A*算法
    plt.figure(1)
    mazerun = plt.gca()
    mazerun.cla()
    mazerun.set_xlim(0, length - 1)
    mazerun.set_ylim(0, width - 1)
    mazerun.xaxis.set_major_locator(ticker.MaxNLocator(length - 1))
    mazerun.yaxis.set_major_locator(ticker.MaxNLocator(width - 1))

    for i in obstacle:  # 画出障碍物和起点终点
        circles = plt.Circle((i[0], i[1]), i[2], color='black')
        mazerun.add_artist(circles)
    plt.fill_between([startPoint[0], startPoint[0] + 1], startPoint[1], startPoint[1] + 1, color='green')
    plt.fill_between([endPoint[0] - 1, endPoint[0]], endPoint[1] - 1, endPoint[1], color='red')

    for i in range(len(pathList)):  # 画出路径点与连线
        plt.plot(pathList[i][0], pathList[i][1], '-k.')
        if i < len(pathList) - 1:
            plt.plot([pathList[i][0], pathList[i + 1][0]], [pathList[i][1], pathList[i + 1][1]], 'r')

    plt.show()


def plotAnimation(obstacle, leaf, newPoint, nearPoint, startPoint, endPoint, length, width):
    plt.figure(2)
    mazerun = plt.gca()
    mazerun.set_xlim(0, length - 1)
    mazerun.set_ylim(0, width - 1)
    mazerun.xaxis.set_major_locator(ticker.MaxNLocator(length - 1))
    mazerun.yaxis.set_major_locator(ticker.MaxNLocator(width - 1))

    for i in obstacle:  # 画出障碍物和起点终点
        circles = plt.Circle((i[0], i[1]), i[2], color='black')
        mazerun.add_artist(circles)
    plt.fill_between([startPoint.x, startPoint.x + 1], startPoint.y, startPoint.y + 1, color='green')
    plt.fill_between([endPoint.x - 1, endPoint.x], endPoint.y - 1, endPoint.y, color='red')

    for node in leaf:
        plt.plot(node.point.x, node.point.y, '-k.')
        if node.ancient != None:
            plt.plot([node.ancient.point.x, node.point.x], [node.ancient.point.y, node.point.y], 'r')

    plt.plot([newPoint.x, nearPoint.x], [newPoint.y, nearPoint.y], 'g')

    plt.show()

    # plt.pause(2)
    # plt.close('all')
    # ani = FuncAnimation(fig, func=update, frames=np.arange(0, 100), blit=True)

def plot3DBase(obstacle,startpoint,endpoint,pointlist):  # 无人车A*算法
    print("??")
    if pointlist == []:
        print("none")
        return
    fig = plt.figure(2)
    ax = plt.subplot(projection='3d')
    x = [d[0] for d in obstacle]
    y = [d[1] for d in obstacle]
    z = [d[2] for d in obstacle]

    ax.bar3d(x,
             y,
             np.zeros_like(z),
             dx=1,
             dy=1,
             dz=z,
             color='red',
             alpha=0.2)
    ax.bar3d([startpoint[0] - 0.5, endpoint[0] - 0.5],
             [startpoint[1] - 0.5, endpoint[1] - 0.5],
             [startpoint[2], endpoint[2]],
             dx=1,
             dy=1,
             dz=1,
             color='green')

    xpos = [c[0] for c in pointlist]
    ypos = [c[1] for c in pointlist]
    zpos = [c[2] for c in pointlist]
    ax.scatter(xpos,
               ypos,
               zpos,
               s=10,
               c='blue')

    ax.plot(xpos,
            ypos,
            zpos,
            c='black',
            label='path')

    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')

    # plt.pause(1)
    print("nihao")
    plt.show()
    # plt.close("all")

def plot3Dmaze(obstacle, startpoint, endpoint):
    fig = plt.figure()
    ax = plt.subplot(projection='3d')
    x = [d[0] for d in obstacle]
    y = [d[1] for d in obstacle]
    z = [d[2] for d in obstacle]

    ax.bar3d(x,
             y,
             np.zeros_like(z),
             dx=1,
             dy=1,
             dz=z,
             color='red')
    ax.bar3d([startpoint[0] - 0.5, endpoint[0] - 0.5],
             [startpoint[1] - 0.5, endpoint[1] - 0.5],
             [startpoint[2], endpoint[2]],
             dx=1,
             dy=1,
             dz=1,
             color='green')
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')

    plt.show()
